AdmittanceController 恒力控制二次开发踩坑记

在开源控制器上做二次开发，「写代码」往往只占排查工作量的一小部分：编译产物不同步、进程环境没刷新、插件发现失效，每一层都能把代码层的正确改动掩盖得毫无反应。本文记录在 ros2_controllers（Humble，v2.54.0 / commit 2105376）的 AdmittanceController 上做恒力控制二次开发的全过程踩坑：fork 出独立包 admittance_controller_fork（原包只读、保留为 diff 基准），按 TDD 注入期望力参数 desired_wrench，把「零力泄放」的导纳改成恒力控制。坑按排查层次排列：代码、编译产物、进程环境、物理语义，全程在 mock 硬件 + GenericSystem 环境验证。控制器架构背景见本站《ros2_control 控制器架构与 chainable 机制》，源码走查见本站《AdmittanceController 源码精读与全流程走查》。

一、代码层

fork 一个 ROS2 包要改五处身份

身份	在哪	改动
包名	CMakeLists.txt 的 project() + package.xml 的 <name>	admittance_controller_fork
pluginlib 插件类名	plugin xml 的 class name/type，测试与 yaml 以字符串引用	admittance_controller_fork/AdmittanceController
C++ namespace	所有 .hpp/.cpp	admittance_controller_fork::
头文件安装路径	include/<pkg>/ 目录 + 所有 #include "<pkg>/..."	include/admittance_controller_fork/
GPL 参数库	generate_parameter_library() 首参 + yaml 顶层 key + 生成头 include	admittance_controller_fork_parameters

两个细节：

• control_msgs/msg/admittance_controller_state 是消息类型名，长得像包名但属于别人，一个字不能动。
• GPL yaml 顶层 key 只决定生成代码的 C++ namespace；参数名前缀（admittance.*）来自组名，与顶层 key 无关。fork 之后原有的在线调参命令一个字不用改。

GPL 生成头的安装位置由 ${PROJECT_NAME}/${LIB_NAME}.hpp 拼出，包名一改，源码里 include 生成头的路径必须跟着改。

批量替换会误伤「文件引用」

sed 词边界规则 s/\badmittance_controller\b/.../g 把 CMakeLists 里 add_library 引用的源文件 src/admittance_controller.cpp 也替换成了 ...fork.cpp，而文件本体按设计保留原名（为了和原包 diff 时同名文件对得上），CMake 报 Cannot find source file。身份标识和文件引用会被同一条规则命中，要么文件跟着改名，要么引用保持原名，两头必须一致。

同名文件粘错包

原包和 fork 的 admittance_rule_impl.hpp 路径只差中间一段，两个编辑器 tab 长得一样，改动粘到了原包。定位三板斧：

stat -c '%y' <文件>          # 时间戳没动 = 没改过
grep -rn 'desired_wrench' .   # 数改动落点
git -C src/ros2_controllers status --short admittance_controller  # 原包纯净度

粘错的同时内容本身还有个前缀笔误（admittance_state_.desired_wrench_，成员挂在 rule 上不在 state 上），粘对了包也编不过。

断言要落在被测行为上（假红）

现象：稳态断言读到的 admittance_position 恒为 0，速度稳定在 0.3333，都和 F_des 无关。

根因在读源码（admittance_rule_impl.hpp:259-271）：admittance_position 是 FK 算出的相对变换——calculate_link_transform(current_joint_pos) 的结果减 ref_trans_base_ft，不是动力学积分器的状态。测试里 current_joint_pos 从建态起就是零向量没人更新，mock 的 FK 又恒返回单位变换，于是 X 恒为 0，弹簧项 K·X 恒为零（控制律在 impl:292：X_ddot = M⁻¹(F − D·Ẋ − K·X)）。系统退化为恒力对纯阻尼，稳态是匀速不是静止：

v = F / (damping + joint_damping) = 5 / (10 + 5) = 0.3333

与实测一位不差。修复：覆盖 mock 的 calculate_link_transform，把积分器状态 state.joint_pos 前三维回灌为末端平移——位置反馈接通，弹簧活过来，「稳态偏移 = 力误差/K」才存在可断言的稳态。

上游测试文件里没有任何稳态偏移用例，原因正在于此：稳态依赖位置反馈回路，identity mock 给不了。测试数值「过分干净」（精确解析值）时要警惕两种可能：断言精确命中了目标语义，或者断言根本没落到新代码路径上——先弄清是哪种。

参数拉取不在数学内核里

apply_parameters_update() 的周期调用在 AdmittanceRule::update()（controller 每周期的主入口，impl:180）里，且受 enable_parameter_update_without_reactivation 开关控制（默认 true，注意它在 yaml 顶层、不带 admittance. 前缀）；calculate_admittance_rule() 是纯数学内核，不拉参数。测试直调内核时，set_parameter 之后要手动补一次 apply_parameters_update()（Testable 子类 using AdmittanceRule::apply_parameters_update; 提权）。

GPL 监听器（生成代码）的更新回调用的是 add_on_set_parameters_callback——同进程内 set_parameter 的调用线程里同步派发，不走 topic、不需要 executor spin。这也是在线调参即时生效的机理。

一个隐蔽点：desired_wrench_ 成员在被 vec_to_eigen 写过之前是未初始化内存，堆上新页恰好全零，控制律减零等于没减。未初始化成员伪装成默认值靠的是运气，不是保证。

二、编译产物层

• colcon test 不编译。改了源码必须先 colcon build，否则测试跑的是旧二进制。判断技巧：失败断言报的行号变了说明重编过。
• 测试链接 build 目录的库，controller_manager dlopen install 目录的库，排查时两份都要 stat 对时间。
• 源码里的 config/launch/urdf 是包的数据文件，launch 按路径读 install 里的副本。改了实验包的 controllers.yaml 后必须 colcon build --packages-select <实验包>。一次故障里 install 的 yaml 停在旧版，type 还是原插件名，CM 一直加载原包的库。

三、进程环境层（其中代价最大的一串）

「改了代码却毫无反应」的完整病根是三个世界不同步：

1. yaml：launch 按文件路径读 install 的副本——改源码要 rebuild 数据包；
2. 插件库：CM 启动时 dlopen，build 之后要重启 launch，活着的进程看不见新 .so；
3. 插件发现：pluginlib 扫 $AMENT_PREFIX_PATH 下各前缀的 share/ament_index/resource_index/controller_interface__pluginlib__plugin/<pkg>——环境变量是 shell source 那一刻的快照。isolated install 的 install/setup.zsh 是聚合脚本，build 新包后旧 shell 不会刷新，直接报 Loader for controller 'admittance_controller' (type 'admittance_controller_fork/AdmittanceController') not found，而 Available classes 列表里只有原包。

排查中两个容易被误导的信号：

• Set parameter successful 没有鉴别力。CM 给控制器节点的 NodeOptions 开着 automatically_declare_parameters_from_overrides（蕴含 allow_undeclared），set 一个不存在的参数会自动声明并返回成功，值躺在参数树里没有任何消费者。参数 get 得到值同样不构成证据。
• /proc/<pid>/maps 是终极证据。进程内存里实际映射的 .so 路径一查便知加载了哪个库：

grep admittance /proc/$(pgrep -f ros2_control_node)/maps | awk '{print $6}' | sort -u

build 新包后的固定配套动作，三件缺一不可：

colcon build --packages-select <pkg>
source install/setup.zsh    # 每个在用的 shell 都要重新 source
# 重启 launch（数据包或插件包有改动时）

四、物理语义层

• mock 的 FT 传感器是 URDF initial_value 写死的常数，GenericSystem::read() 每周期原样回放，与臂的位置无关——环境反馈回路不存在，等效环境刚度为 0。
• 因此 mock 里恒力控制的验收标准是偏移重新平衡：x* = (F_meas − F_des)/K。力误差恒存不妨碍静止——稳态条件是合力为零，弹簧力 K·x* 顶住误差即可，不需要每个力消失。导纳没有积分器，弹簧就是位移限位，误差再大偏移也封顶在 F_err/K；会一直跑到头的是带积分的控制器（windup）。这是导纳式力控相对积分型力控的安全属性。
• 方向与坐标系：实测力 wrench_base = (−9.13, 0, −4.08) N（base 帧，模长 10 N，沿传感器 −z 的投影）；desired_wrench 表达在 control frame（tool0，与传感器帧平行），同一个力写成 (0, 0, −10)。设参数前先 ros2 topic echo 确认符号，这是真机检查单的第一条。验收：设 [0,0,−10]（与实测同向同值）偏移从 0.1 m 收回 0；设 [0,0,−20] 偏移反向漂 0.1 m。

要点

• fork 包的身份五件套加上同形消息名，动手前一次列全；批量替换同时命中身份标识与文件引用，CMake 的源文件引用单独核对。
• 假测试的两个识别信号：断言读到的量不随被测行为变化（FK 量 vs 积分量）；mock 没有接通被测机制依赖的反馈（弹簧需要位置反馈）。
• 「改了没反应」先查三个世界：源码到 install（rebuild）、install 到进程（重启）、环境到注册表（重新 source）。Set parameter successful 不构成任何证据，/proc/<pid>/maps 才是。
• mock 验收看偏移平衡，不看力收敛——传感器是死值，环境回路本来就不存在。

导纳定律的坐标系关系（六个位姿的来历）见本站《AdmittanceTransforms 的六个位姿：导纳定律的坐标桥梁》；力控算法层面导纳与 PI 的对比见本站《导纳控制与 PI 控制：力控算法的定位与对比》。